/*
 * Copyright (c) Huawei Technologies Co., Ltd. 2024-2024.
 * Description: non-linefetch fault dfx functions
 * Author: wanglinhui <wanglinhui@huawei.com>
 * Create: 2024-05-30
 */
#include <linux/init.h>
#include <linux/signal.h>
#include <internal/rtos_extable.h>

#ifdef __ARMEB__
#define RTOS_BE		1
#else
#define RTOS_BE		0
#endif

#define rtos_get8_unaligned_check(ins, val, addr, err)	\
	__asm__(					\
	ARM("1:	"ins"	%1, [%2], #1\n")		\
	THUMB("1:	"ins"	%1, [%2]\n")		\
	THUMB("	add	%2, %2, #1\n")		\
	"2:\n"						\
	"	.pushsection .text.fixup,\"ax\"\n"	\
	"	.align	2\n"				\
	"3:	mov	%0, #1\n"			\
	"	b	2b\n"				\
	"	.popsection\n"				\
	"	rtos_ex_entry	1b, 3b\n"		\
	: "=r" (err), "=&r" (val), "=r" (addr)		\
	: "0" (err), "2" (addr))

static bool non_linefetch_dfx_enhance;
static int __init set_non_linefetch_dfx_enhance(char *str)
{
	non_linefetch_dfx_enhance = true;
	pr_info("non-linefetch dfx enhance enabled\n");
	return 0;
}
__setup("non_linefetch_dfx_enhance", set_non_linefetch_dfx_enhance);

bool get_non_linefetch_dfx_enhance(void)
{
	return non_linefetch_dfx_enhance;
}

static int do_alignment_bad(unsigned long addr, unsigned int fsr, struct pt_regs *regs)
{
	if (fixup_exception_rtos(regs))
		return 0;

	return 1;
}

void __init init_non_linefetch_fault_code(void)
{
	if (!non_linefetch_dfx_enhance)
		return;

	/* 8 is "external abort on non-linefetch" index in fsr_info struct */
	hook_fault_code(8, do_alignment_bad, SIGBUS, BUS_ADRALN,
		"external abort on non-linefetch");
	/* 10 is "external abort on non-linefetch" index in fsr_info struct */
	hook_fault_code(10, do_alignment_bad, SIGBUS, BUS_ADRALN,
		"external abort on non-linefetch");
}

int rtos_get32t_unaligned_check(unsigned int *val, unsigned long addr)
{
	unsigned int err = 0;
	unsigned int v;
	unsigned int a = addr;

	rtos_get8_unaligned_check("ldrbt", v, a, err);
	*val = v << ((RTOS_BE) ? 24 : 0);
	rtos_get8_unaligned_check("ldrbt", v, a, err);
	*val |= v << ((RTOS_BE) ? 16 : 8);
	rtos_get8_unaligned_check("ldrbt", v, a, err);
	*val |= v << ((RTOS_BE) ? 8 : 16);
	rtos_get8_unaligned_check("ldrbt", v, a, err);
	*val |= v << ((RTOS_BE) ? 0 : 24);
	if (err)
		return -1;

	return 0;
}
